#include "arm_control/arm_action_type5.hpp"
#include <chrono>
#include <thread>

//扫码动作

namespace arm_control
{

ArmActionType5::ArmActionType5(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 初始化MoveGroupInterface，左机械臂
  move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_left");
  
  // 创建爪子发布器
  zhuazi_left_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_left", 10);
  
  RCLCPP_INFO(logger_, "动作类型5初始化完成");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化，Planning Group: arm_left");
  RCLCPP_INFO(logger_, "爪子发布器已创建: /zhuazi_left");
}

void ArmActionType5::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型5 - 左臂移动到扫码位置");
  
  // 先将xuan12运动到0.000（过渡状态）
  feedback->progress_percentage = 5.0;
  feedback->current_status = "将xuan12移动到初始位置...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "将xuan12设置为0.000");
  
  move_group_left_->setStartStateToCurrentState();
  
  // 获取当前状态并修改xuan12关节
  moveit::core::RobotStatePtr current_state = move_group_left_->getCurrentState(10.0);
  if (!current_state) {
    RCLCPP_ERROR(logger_, "无法获取当前机器人状态");
    result->success = false;
    result->message = "动作类型5失败 - 无法获取当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取所有关节的当前值
  std::vector<double> joint_group_positions;
  const moveit::core::JointModelGroup* joint_model_group = 
    current_state->getJointModelGroup("arm_left");
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
  
  // 只修改xuan12的值
  const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
  for (size_t i = 0; i < joint_names.size(); i++) {
    if (joint_names[i] == "xuan12") {
      joint_group_positions[i] = 0.000;
      RCLCPP_INFO(logger_, "找到xuan12关节，设置为0.000");
      break;
    }
  }
  
  move_group_left_->setJointValueTarget(joint_group_positions);
  
  moveit::planning_interface::MoveGroupInterface::Plan xuan12_plan;
  bool xuan12_success = (move_group_left_->plan(xuan12_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!xuan12_success) {
    result->success = false;
    result->message = "动作类型5失败 - xuan12路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "xuan12路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "xuan12路径规划成功");
  
  auto xuan12_execute_result = move_group_left_->execute(xuan12_plan);
  if (xuan12_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型5失败 - xuan12运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "xuan12运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "xuan12已到达初始位置");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型5已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型5已取消");
    return;
  }
  
  // 左臂运动到scan_QR_pose
  feedback->progress_percentage = 10.0;
  feedback->current_status = "左臂移动到scan_QR_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置左臂目标姿态为scan_QR_pose");
  
  move_group_left_->setNamedTarget("scan_QR_pose");
  move_group_left_->setStartStateToCurrentState();
  
  moveit::planning_interface::MoveGroupInterface::Plan left_plan;
  bool left_success = (move_group_left_->plan(left_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!left_success) {
    result->success = false;
    result->message = "动作类型5失败 - 左臂到scan_QR_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到scan_QR_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂到scan_QR_pose路径规划成功");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型5已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型5已取消");
    return;
  }
  
  feedback->progress_percentage = 20.0;
  feedback->current_status = "左臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto left_execute_result = move_group_left_->execute(left_plan);
  if (left_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型5失败 - 左臂到scan_QR_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到scan_QR_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂已到达scan_QR_pose");
  
  // 延时1秒
  RCLCPP_INFO(logger_, "到达scan_QR_pose后延时1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型5已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型5已取消");
    return;
  }
  
  // 打开左爪子
  feedback->progress_percentage = 30.0;
  feedback->current_status = "打开左爪子...";
  goal_handle->publish_feedback(feedback);
  
  auto left_msg = std_msgs::msg::Int32();
  left_msg.data = 120;
  zhuazi_left_publisher_->publish(left_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_left: 120 (打开)");
  
  // 等待爪子动作完成
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型5已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型5已取消");
    return;
  }
  
  // 等待机械臂稳定
  feedback->progress_percentage = 90.0;
  feedback->current_status = "等待机械臂稳定...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "等待2秒让机械臂稳定...");
  std::this_thread::sleep_for(std::chrono::seconds(2));
  
  // 动作完成
  feedback->progress_percentage = 100.0;
  feedback->current_status = "扫码任务完成";
  goal_handle->publish_feedback(feedback);
  
  result->success = true;
  result->message = "动作类型5执行成功 - 左臂已到达扫码位置";
  goal_handle->succeed(result);
  RCLCPP_INFO(logger_, "动作类型5执行成功 - 左臂已到达扫码位置");
}

} // namespace arm_control

